/******************************************************************************
 *  _____     _____
 *  \  __\    \  __\
 *   \ \_\\    \ \_\\
 *    \  __\SS  \  __\roject
 *     \_\       \_\
 *
 * Mateusz Maciąg, Rafał Urbański
 * Komputerowe Systemy Sterowania
 *****************************************************************************/

#include "DiscreteObjectParams.h"

#include <stdexcept>
#include <sstream>
#include <cmath>

#include "SimEngine/DiscreteObject.h"
#include "Utils.h"

namespace RafMat
{
namespace SimEngine
{

using namespace std;
using namespace SimInterface;

// Stałe łańcuchy znaków
const string DiscreteObjectParams::CONF_APOLY("apoly");
const string DiscreteObjectParams::CONF_BPOLY("bpoly");
const string DiscreteObjectParams::CONF_CPOLY("cpoly");
const string DiscreteObjectParams::CONF_AGPOLY("agpoly");
const string DiscreteObjectParams::CONF_D("d");
const string DiscreteObjectParams::CONF_DELAY("delay");
const string DiscreteObjectParams::CONF_SWITCH_TIME("switch_time");

DiscreteObjectParams::DiscreteObjectParams(const DiscreteObject &owner,
                                           Polynomial a,
                                           Polynomial b,
                                           Polynomial c,
                                           Polynomial ag,
                                           double d,
                                           unsigned int delay, int switchTime)
    : IConfigurable("ARXParams"),
      m_owner(owner), m_a(a), m_b(b), m_c(c), m_ag(ag), m_d(d),m_delay(delay),
      m_switchTime(switchTime), m_noiseGain(1), m_deltaGain(1)
{
    reset();
}

DiscreteObjectParams::DiscreteObjectParams(const DiscreteObject &owner,
                                           PolyType va,
                                           PolyType vb,
                                           PolyType vc,
                                           PolyType vag,
                                           unsigned int d,
                                           unsigned int delay, int switchTime)
    : IConfigurable("ARXParams"),
      m_owner(owner), m_a(Polynomial(va)), m_b(Polynomial(vb)), m_c(Polynomial(vc)),m_ag(Polynomial(vag)),
      m_d(d),m_delay(delay), m_switchTime(switchTime), m_noiseGain(1), m_deltaGain(1)
{
    reset();
}

void DiscreteObjectParams::reset()
{
    // Wielomiany trzeba przygotować tak, by a0 = 1.
    // Dla a != a0 otrzymujemy równanie y(i) * a0 = ..., więc trzeba dzielić
    // obustronnie przez a0 (włącznie z szumem).

    if(abs(m_a[0]) < 1e-200)
        throw std::domain_error("ARX transfer function must have non-zero constant term in denominator.");

    double invA0 = 1.0/m_a[0];

    // Skracanie funkcji wymiernej przez wyraz wolny.
    m_a *= invA0;
    m_b *= invA0;
    m_c *= invA0;

    // ----- RAF
    // Wielomian Ag ma miec ag(0) = 1

    if(abs(m_ag[0]) < 1e-200)
        throw std::domain_error("ARX transfer function must have non-zero constant term in denominator. -- AG");

    double invAG0 = 1.0/m_ag[0];

    // Skracanie funkcji wymiernej przez wyraz wolny.
    m_ag *= invAG0;

    m_deltaGain = invAG0;
    m_d*=m_deltaGain;
    //-----END-----

    // Skracanie wypływa też na wzmocnienie białego szumu (dzielimy cały wzór na y(i) przez a0)
    m_noiseGain = invA0;
}

void DiscreteObjectParams::setTransferFcn(const Polynomial& Ag, double d,const Polynomial& C,const Polynomial& B, const Polynomial& A, unsigned delay)
{
    unsigned maxDelay = m_owner.getMaxDelay();
    unsigned maxD = m_owner.getMaxD();
    unsigned maxA = m_owner.getAMaxRank();
    unsigned maxB = m_owner.getBMaxRank();
    unsigned maxC = m_owner.getCMaxRank();
    unsigned maxAg = m_owner.getAgMaxRank();

    if(maxDelay < delay)
        throw iconf_error::tooHighDelay(*this, CONF_DELAY, maxDelay, delay);
    else if(maxA < A.degree())
        throw iconf_error::tooHighDegree(*this, CONF_APOLY, maxA, A.degree());
    else if(maxB < B.degree())
        throw iconf_error::tooHighDegree(*this, CONF_BPOLY, maxB, B.degree());
    else if(maxC < C.degree())
        throw iconf_error::tooHighDegree(*this, CONF_CPOLY, maxC, C.degree());
    else if(maxAg < Ag.degree())
        throw iconf_error::tooHighDegree(*this, CONF_AGPOLY, maxAg, Ag.degree());
    if(maxD < d)
        throw iconf_error::tooHighDelay(*this, CONF_D, maxD, d);

    this->m_a = A;
    this->m_b = B;
    this->m_c = C;
    this->m_ag = Ag;
    this->m_d = d;
    this->m_delay = delay;
    reset();
}

//Tutaj nie zadziala bo RMNK nie zwraca C!!!! Zeby dzialalo dalem dwa razy wielomian B
void DiscreteObjectParams::setTransferFcn(WrmnkARXWrapperSPtr fromRmnk)
{
    this->setTransferFcn(1,0,0, fromRmnk->getEstimatedB(), fromRmnk->getEstimatedA(), fromRmnk->getDelay());
}

//
// Konfiguracja parametrów z poziomu GIU
//

bool DiscreteObjectParams::setParameter(const string& param, const string& value) throw(std::logic_error)
{
    // Usuń ewentualne wiodące i końcowe spacje
    string tparam = Utils::trimWhiteSpaces(param);
    string tvalue = Utils::trimWhiteSpaces(value);

    // Strumień danych dla łańcucha argumentu.
    istringstream istr(tvalue);

    bool success = true;

    if(tparam == CONF_DELAY)
    {
        // Odczytaj wartość opóźnienia obiektu i sprawdź czy nie jest zbyt duża.
        unsigned int tmpDly;
        istr >> tmpDly;
        if(!istr.bad())
        {
            unsigned int maxDly = m_owner.getMaxDelay();
            if( tmpDly > maxDly )
                throw iconf_error::tooHighDelay(*this, tvalue, maxDly, tmpDly);

            m_delay = tmpDly;
        }
    }
    else if(tparam == CONF_APOLY)
    {
        Polynomial poly = Polynomial(Utils::convertStrToVector(tvalue, /* out */ success));
        if(success)
        {
            int givenDegree = poly.degree(), maxDegree = m_owner.getAMaxRank();
            if(givenDegree > maxDegree)
                throw iconf_error::tooHighDegree(*this, tvalue, maxDegree, givenDegree);

            m_a = poly;
            reset();
        }
    }
    else if(tparam == CONF_BPOLY)
    {
        Polynomial poly = Polynomial(Utils::convertStrToVector(tvalue, /* out */ success));
        if(success)
        {
            int givenDegree = poly.degree(), maxDegree = m_owner.getBMaxRank();
            if(givenDegree > maxDegree)
                throw iconf_error::tooHighDegree(*this, tvalue, maxDegree, givenDegree);

            m_b = poly;
            reset();
        }
    }
    else if(tparam == CONF_CPOLY)
    {
        Polynomial poly = Polynomial(Utils::convertStrToVector(tvalue, /* out */ success));
        if(success)
        {
            int givenDegree = poly.degree(), maxDegree = m_owner.getCMaxRank();
            if(givenDegree > maxDegree)
                throw iconf_error::tooHighDegree(*this, tvalue, maxDegree, givenDegree);

            m_c = poly;
            reset();
        }
    }
    else if(tparam == CONF_AGPOLY)
    {
        Polynomial poly = Polynomial(Utils::convertStrToVector(tvalue, /* out */ success));
        if(success)
        {
            int givenDegree = poly.degree(), maxDegree = m_owner.getAgMaxRank();
            if(givenDegree > maxDegree)
                throw iconf_error::tooHighDegree(*this, tvalue, maxDegree, givenDegree);

            m_ag = poly;
            reset();
        }
    }
    else if(tparam == CONF_D)
    {
        // Odczytaj wartość opóźnienia obiektu i sprawdź czy nie jest zbyt duża.
        double tmpD;
        istr >> tmpD;
        if(!istr.bad())
        {
            unsigned int maxD = m_owner.getMaxD();
            if( tmpD > maxD )
                throw iconf_error::tooHighD(*this, tvalue, maxD, tmpD);

            m_d = tmpD;
        }
    }
    else if(param == CONF_SWITCH_TIME)
    {
        int tmpSwTime;
        istr >> tmpSwTime;
        if(!istr.bad())
        {
            if(tmpSwTime < -1)
                throw iconf_error::badSwitchTime(*this, tvalue, tmpSwTime);
            m_switchTime = tmpSwTime;
        }
    }
    else
        throw iconf_error::unrecognizedParam(*this, tparam);

    if(istr.bad() || !success)
        throw iconf_error::syntaxError(*this, tvalue);

    return true;
}

std::string DiscreteObjectParams::getParameter(const string& param) const throw(std::logic_error)
{
    typedef ostream_iterator<double> oit;

    string tparam = Utils::trimWhiteSpaces(param);

    string tmpOut;
    ostringstream ostr(tmpOut);

    if(tparam == CONF_DELAY)
    {
        ostr << m_delay;
    }
    else if(tparam == CONF_D)
    {
        ostr << m_d/m_deltaGain;
    }
    else if(tparam == CONF_APOLY)
    {
        Polynomial tmpPoly = m_a;
        tmpPoly /= m_noiseGain;
        ostr << "[ ";
        copy(tmpPoly.begin(), tmpPoly.end(), oit(ostr, " "));
        ostr << ']';
    }
    else if(tparam == CONF_BPOLY)
    {
        Polynomial tmpPoly = m_b;
        tmpPoly /= m_noiseGain;
        ostr << "[ ";
        copy(tmpPoly.begin(), tmpPoly.end(), oit(ostr, " "));
        ostr << ']';
    }
    else if(tparam == CONF_CPOLY)
    {
        Polynomial tmpPoly = m_c;
        tmpPoly /= m_noiseGain;
        ostr << "[ ";
        copy(tmpPoly.begin(), tmpPoly.end(), oit(ostr, " "));
        ostr << ']';
    }
    else if(tparam == CONF_AGPOLY)
    {
        Polynomial tmpPoly = m_ag;
        tmpPoly /= m_deltaGain;
        ostr << "[ ";
        copy(tmpPoly.begin(), tmpPoly.end(), oit(ostr, " "));
        ostr << ']';
    }
    else if(tparam == CONF_SWITCH_TIME)
    {
        ostr << m_switchTime;
    }
    else
        throw iconf_error::unrecognizedParam(*this, tparam);

    return ostr.str();
}

SimFile::ConfigWriter& DiscreteObjectParams::dumpParameters(SimFile::ConfigWriter& o) const
{
    if(m_switchTime >= -1) // -2 jest wartością zarezerwowaną i nie może się znaleźć w pliku
    {
        o.pushBlock(SimFile::ConfigWriter::BlockParams, getName());
        o.pushParam(CONF_DELAY, m_delay);
        o.pushParam(CONF_D, m_d/m_deltaGain);
        o.pushParam(CONF_AGPOLY, m_ag / m_deltaGain);
        o.pushParam(CONF_CPOLY, m_c / m_noiseGain);
        o.pushParam(CONF_BPOLY, m_b / m_noiseGain);
        o.pushParam(CONF_APOLY, m_a / m_noiseGain);
        o.pushParam(CONF_SWITCH_TIME, m_switchTime);
        o.closeBlock();
    }

    return o;
}

}   // SimEngine namespace
}   // RafMat namespace
